/*************************************************************************
Copyright (c) 1992-2007 The University of Tennessee.  All rights reserved.

Contributors:
    * Sergey Bochkanov (ALGLIB project). Translation from FORTRAN to
      pseudocode.

See subroutines comments for additional copyrights.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

- Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.

- Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer listed
  in this license in the documentation and/or other materials
  provided with the distribution.

- Neither the name of the copyright holders nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/

#include <stdafx.h>
#include "sinverse.h"

/*************************************************************************
Inversion of a symmetric indefinite matrix

The algorithm gets an LDLT-decomposition as an input, generates matrix A^-1
and saves the lower or upper triangle of an inverse matrix depending on the
input (U*D*U' or L*D*L').

Input parameters:
    A       -   LDLT-decomposition of the matrix,
                Output of subroutine SMatrixLDLT.
    N       -   size of matrix A.
    IsUpper -   storage format. If IsUpper = True, then the symmetric matrix
                is given as decomposition A = U*D*U' and this decomposition
                is stored in the upper triangle of matrix A and on the main
                diagonal, and the lower triangle of matrix A is not used.
    Pivots  -   a table of permutations, output of subroutine SMatrixLDLT.

Output parameters:
    A       -   inverse of the matrix, whose LDLT-decomposition was stored
                in matrix A as a subroutine input.
                Array with elements [0..N-1, 0..N-1].
                If IsUpper = True, then A contains the upper triangle of
                matrix A^-1, and the elements below the main diagonal are
                not used nor changed. The same applies if IsUpper = False.

Result:
    True, if the matrix is not singular.
    False, if the matrix is singular and could not be inverted.

  -- LAPACK routine (version 3.0) --
     Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
     Courant Institute, Argonne National Lab, and Rice University
     March 31, 1993
*************************************************************************/
bool smatrixldltinverse(ap::real_2d_array& a,
     const ap::integer_1d_array& pivots,
     int n,
     bool isupper)
{
    bool result;
    ap::real_1d_array work;
    ap::real_1d_array work2;
    int i;
    int k;
    int kp;
    int kstep;
    double ak;
    double akkp1;
    double akp1;
    double d;
    double t;
    double temp;
    int km1;
    int kp1;
    int l;
    int i1;
    int i2;
    double v;

    work.setbounds(1, n);
    work2.setbounds(1, n);
    result = true;
    
    //
    // Quick return if possible
    //
    if( n==0 )
    {
        return result;
    }
    
    //
    // Check that the diagonal matrix D is nonsingular.
    //
    for(i = 0; i <= n-1; i++)
    {
        if( pivots(i)>=0&&a(i,i)==0 )
        {
            result = false;
            return result;
        }
    }
    if( isupper )
    {
        
        //
        // Compute inv(A) from the factorization A = U*D*U'.
        //
        // K+1 is the main loop index, increasing from 1 to N in steps of
        // 1 or 2, depending on the size of the diagonal blocks.
        //
        k = 0;
        while(k<=n-1)
        {
            if( pivots(k)>=0 )
            {
                
                //
                // 1 x 1 diagonal block
                //
                // Invert the diagonal block.
                //
                a(k,k) = 1/a(k,k);
                
                //
                // Compute column K+1 of the inverse.
                //
                if( k>0 )
                {
                    ap::vmove(work.getvector(1, k), a.getcolumn(k, 0, k-1));
                    symmetricmatrixvectormultiply(a, isupper, 1-1, k+1-1-1, work, double(-1), work2);
                    ap::vmove(a.getcolumn(k, 0, k-1), work2.getvector(1, k));
                    v = ap::vdotproduct(&work2(1), &work(1), ap::vlen(1,k));
                    a(k,k) = a(k,k)-v;
                }
                kstep = 1;
            }
            else
            {
                
                //
                // 2 x 2 diagonal block
                //
                // Invert the diagonal block.
                //
                t = fabs(a(k,k+1));
                ak = a(k,k)/t;
                akp1 = a(k+1,k+1)/t;
                akkp1 = a(k,k+1)/t;
                d = t*(ak*akp1-1);
                a(k,k) = akp1/d;
                a(k+1,k+1) = ak/d;
                a(k,k+1) = -akkp1/d;
                
                //
                // Compute columns K+1 and K+1+1 of the inverse.
                //
                if( k>0 )
                {
                    ap::vmove(work.getvector(1, k), a.getcolumn(k, 0, k-1));
                    symmetricmatrixvectormultiply(a, isupper, 0, k-1, work, double(-1), work2);
                    ap::vmove(a.getcolumn(k, 0, k-1), work2.getvector(1, k));
                    v = ap::vdotproduct(&work(1), &work2(1), ap::vlen(1,k));
                    a(k,k) = a(k,k)-v;
                    v = ap::vdotproduct(a.getcolumn(k, 0, k-1), a.getcolumn(k+1, 0, k-1));
                    a(k,k+1) = a(k,k+1)-v;
                    ap::vmove(work.getvector(1, k), a.getcolumn(k+1, 0, k-1));
                    symmetricmatrixvectormultiply(a, isupper, 0, k-1, work, double(-1), work2);
                    ap::vmove(a.getcolumn(k+1, 0, k-1), work2.getvector(1, k));
                    v = ap::vdotproduct(&work(1), &work2(1), ap::vlen(1,k));
                    a(k+1,k+1) = a(k+1,k+1)-v;
                }
                kstep = 2;
            }
            if( pivots(k)>=0 )
            {
                kp = pivots(k);
            }
            else
            {
                kp = n+pivots(k);
            }
            if( kp!=k )
            {
                
                //
                // Interchange rows and columns K and KP in the leading
                // submatrix
                //
                ap::vmove(work.getvector(1, kp), a.getcolumn(k, 0, kp-1));
                ap::vmove(a.getcolumn(k, 0, kp-1), a.getcolumn(kp, 0, kp-1));
                ap::vmove(a.getcolumn(kp, 0, kp-1), work.getvector(1, kp));
                ap::vmove(work.getvector(1, k-1-kp), a.getcolumn(k, kp+1, k-1));
                ap::vmove(a.getcolumn(k, kp+1, k-1), a.getrow(kp, kp+1, k-1));
                ap::vmove(&a(kp, kp+1), &work(1), ap::vlen(kp+1,k-1));
                temp = a(k,k);
                a(k,k) = a(kp,kp);
                a(kp,kp) = temp;
                if( kstep==2 )
                {
                    temp = a(k,k+1);
                    a(k,k+1) = a(kp,k+1);
                    a(kp,k+1) = temp;
                }
            }
            k = k+kstep;
        }
    }
    else
    {
        
        //
        // Compute inv(A) from the factorization A = L*D*L'.
        //
        // K is the main loop index, increasing from 0 to N-1 in steps of
        // 1 or 2, depending on the size of the diagonal blocks.
        //
        k = n-1;
        while(k>=0)
        {
            if( pivots(k)>=0 )
            {
                
                //
                // 1 x 1 diagonal block
                //
                // Invert the diagonal block.
                //
                a(k,k) = 1/a(k,k);
                
                //
                // Compute column K+1 of the inverse.
                //
                if( k<n-1 )
                {
                    ap::vmove(work.getvector(1, n-k-1), a.getcolumn(k, k+1, n-1));
                    symmetricmatrixvectormultiply(a, isupper, k+1, n-1, work, double(-1), work2);
                    ap::vmove(a.getcolumn(k, k+1, n-1), work2.getvector(1, n-k-1));
                    v = ap::vdotproduct(&work(1), &work2(1), ap::vlen(1,n-k-1));
                    a(k,k) = a(k,k)-v;
                }
                kstep = 1;
            }
            else
            {
                
                //
                // 2 x 2 diagonal block
                //
                // Invert the diagonal block.
                //
                t = fabs(a(k,k-1));
                ak = a(k-1,k-1)/t;
                akp1 = a(k,k)/t;
                akkp1 = a(k,k-1)/t;
                d = t*(ak*akp1-1);
                a(k-1,k-1) = akp1/d;
                a(k,k) = ak/d;
                a(k,k-1) = -akkp1/d;
                
                //
                // Compute columns K+1-1 and K+1 of the inverse.
                //
                if( k<n-1 )
                {
                    ap::vmove(work.getvector(1, n-k-1), a.getcolumn(k, k+1, n-1));
                    symmetricmatrixvectormultiply(a, isupper, k+1, n-1, work, double(-1), work2);
                    ap::vmove(a.getcolumn(k, k+1, n-1), work2.getvector(1, n-k-1));
                    v = ap::vdotproduct(&work(1), &work2(1), ap::vlen(1,n-k-1));
                    a(k,k) = a(k,k)-v;
                    v = ap::vdotproduct(a.getcolumn(k, k+1, n-1), a.getcolumn(k-1, k+1, n-1));
                    a(k,k-1) = a(k,k-1)-v;
                    ap::vmove(work.getvector(1, n-k-1), a.getcolumn(k-1, k+1, n-1));
                    symmetricmatrixvectormultiply(a, isupper, k+1, n-1, work, double(-1), work2);
                    ap::vmove(a.getcolumn(k-1, k+1, n-1), work2.getvector(1, n-k-1));
                    v = ap::vdotproduct(&work(1), &work2(1), ap::vlen(1,n-k-1));
                    a(k-1,k-1) = a(k-1,k-1)-v;
                }
                kstep = 2;
            }
            if( pivots(k)>=0 )
            {
                kp = pivots(k);
            }
            else
            {
                kp = pivots(k)+n;
            }
            if( kp!=k )
            {
                
                //
                // Interchange rows and columns K and KP
                //
                if( kp<n-1 )
                {
                    ap::vmove(work.getvector(1, n-kp-1), a.getcolumn(k, kp+1, n-1));
                    ap::vmove(a.getcolumn(k, kp+1, n-1), a.getcolumn(kp, kp+1, n-1));
                    ap::vmove(a.getcolumn(kp, kp+1, n-1), work.getvector(1, n-kp-1));
                }
                ap::vmove(work.getvector(1, kp-k-1), a.getcolumn(k, k+1, kp-1));
                ap::vmove(a.getcolumn(k, k+1, kp-1), a.getrow(kp, k+1, kp-1));
                ap::vmove(&a(kp, k+1), &work(1), ap::vlen(k+1,kp-1));
                temp = a(k,k);
                a(k,k) = a(kp,kp);
                a(kp,kp) = temp;
                if( kstep==2 )
                {
                    temp = a(k,k-1);
                    a(k,k-1) = a(kp,k-1);
                    a(kp,k-1) = temp;
                }
            }
            k = k-kstep;
        }
    }
    return result;
}


/*************************************************************************
Inversion of a symmetric indefinite matrix

Given a lower or upper triangle of matrix A, the algorithm generates
matrix A^-1 and saves the lower or upper triangle depending on the input.

Input parameters:
    A       -   matrix to be inverted (upper or lower triangle).
                Array with elements [0..N-1, 0..N-1].
    N       -   size of matrix A.
    IsUpper -   storage format. If IsUpper = True, then the upper
                triangle of matrix A is given, otherwise the lower
                triangle is given.

Output parameters:
    A       -   inverse of matrix A.
                Array with elements [0..N-1, 0..N-1].
                If IsUpper = True, then A contains the upper triangle of
                matrix A^-1, and the elements below the main diagonal are
                not used nor changed.
                The same applies if IsUpper = False.

Result:
    True, if the matrix is not singular.
    False, if the matrix is singular and could not be inverted.

  -- LAPACK routine (version 3.0) --
     Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
     Courant Institute, Argonne National Lab, and Rice University
     March 31, 1993
*************************************************************************/
bool smatrixinverse(ap::real_2d_array& a, int n, bool isupper)
{
    bool result;
    ap::integer_1d_array pivots;

    smatrixldlt(a, n, isupper, pivots);
    result = smatrixldltinverse(a, pivots, n, isupper);
    return result;
}


/*************************************************************************
Obsolete 1-based subroutine
*************************************************************************/
bool inverseldlt(ap::real_2d_array& a,
     const ap::integer_1d_array& pivots,
     int n,
     bool isupper)
{
    bool result;
    ap::real_1d_array work;
    ap::real_1d_array work2;
    int i;
    int k;
    int kp;
    int kstep;
    double ak;
    double akkp1;
    double akp1;
    double d;
    double t;
    double temp;
    int km1;
    int kp1;
    int l;
    int i1;
    int i2;
    double v;

    work.setbounds(1, n);
    work2.setbounds(1, n);
    result = true;
    
    //
    // Quick return if possible
    //
    if( n==0 )
    {
        return result;
    }
    
    //
    // Check that the diagonal matrix D is nonsingular.
    //
    for(i = 1; i <= n; i++)
    {
        if( pivots(i)>0&&a(i,i)==0 )
        {
            result = false;
            return result;
        }
    }
    if( isupper )
    {
        
        //
        // Compute inv(A) from the factorization A = U*D*U'.
        //
        // K is the main loop index, increasing from 1 to N in steps of
        // 1 or 2, depending on the size of the diagonal blocks.
        //
        k = 1;
        while(k<=n)
        {
            if( pivots(k)>0 )
            {
                
                //
                // 1 x 1 diagonal block
                //
                // Invert the diagonal block.
                //
                a(k,k) = 1/a(k,k);
                
                //
                // Compute column K of the inverse.
                //
                if( k>1 )
                {
                    km1 = k-1;
                    ap::vmove(work.getvector(1, km1), a.getcolumn(k, 1, km1));
                    symmetricmatrixvectormultiply(a, isupper, 1, k-1, work, double(-1), work2);
                    ap::vmove(a.getcolumn(k, 1, km1), work2.getvector(1, km1));
                    v = ap::vdotproduct(&work2(1), &work(1), ap::vlen(1,km1));
                    a(k,k) = a(k,k)-v;
                }
                kstep = 1;
            }
            else
            {
                
                //
                // 2 x 2 diagonal block
                //
                // Invert the diagonal block.
                //
                t = fabs(a(k,k+1));
                ak = a(k,k)/t;
                akp1 = a(k+1,k+1)/t;
                akkp1 = a(k,k+1)/t;
                d = t*(ak*akp1-1);
                a(k,k) = akp1/d;
                a(k+1,k+1) = ak/d;
                a(k,k+1) = -akkp1/d;
                
                //
                // Compute columns K and K+1 of the inverse.
                //
                if( k>1 )
                {
                    km1 = k-1;
                    kp1 = k+1;
                    ap::vmove(work.getvector(1, km1), a.getcolumn(k, 1, km1));
                    symmetricmatrixvectormultiply(a, isupper, 1, k-1, work, double(-1), work2);
                    ap::vmove(a.getcolumn(k, 1, km1), work2.getvector(1, km1));
                    v = ap::vdotproduct(&work(1), &work2(1), ap::vlen(1,km1));
                    a(k,k) = a(k,k)-v;
                    v = ap::vdotproduct(a.getcolumn(k, 1, km1), a.getcolumn(kp1, 1, km1));
                    a(k,k+1) = a(k,k+1)-v;
                    ap::vmove(work.getvector(1, km1), a.getcolumn(kp1, 1, km1));
                    symmetricmatrixvectormultiply(a, isupper, 1, k-1, work, double(-1), work2);
                    ap::vmove(a.getcolumn(kp1, 1, km1), work2.getvector(1, km1));
                    v = ap::vdotproduct(&work(1), &work2(1), ap::vlen(1,km1));
                    a(k+1,k+1) = a(k+1,k+1)-v;
                }
                kstep = 2;
            }
            kp = abs(pivots(k));
            if( kp!=k )
            {
                
                //
                // Interchange rows and columns K and KP in the leading
                // submatrix A(1:k+1,1:k+1)
                //
                l = kp-1;
                ap::vmove(work.getvector(1, l), a.getcolumn(k, 1, l));
                ap::vmove(a.getcolumn(k, 1, l), a.getcolumn(kp, 1, l));
                ap::vmove(a.getcolumn(kp, 1, l), work.getvector(1, l));
                l = k-kp-1;
                i1 = kp+1;
                i2 = k-1;
                ap::vmove(work.getvector(1, l), a.getcolumn(k, i1, i2));
                ap::vmove(a.getcolumn(k, i1, i2), a.getrow(kp, i1, i2));
                ap::vmove(&a(kp, i1), &work(1), ap::vlen(i1,i2));
                temp = a(k,k);
                a(k,k) = a(kp,kp);
                a(kp,kp) = temp;
                if( kstep==2 )
                {
                    temp = a(k,k+1);
                    a(k,k+1) = a(kp,k+1);
                    a(kp,k+1) = temp;
                }
            }
            k = k+kstep;
        }
    }
    else
    {
        
        //
        // Compute inv(A) from the factorization A = L*D*L'.
        //
        // K is the main loop index, increasing from 1 to N in steps of
        // 1 or 2, depending on the size of the diagonal blocks.
        //
        k = n;
        while(k>=1)
        {
            if( pivots(k)>0 )
            {
                
                //
                // 1 x 1 diagonal block
                //
                // Invert the diagonal block.
                //
                a(k,k) = 1/a(k,k);
                
                //
                // Compute column K of the inverse.
                //
                if( k<n )
                {
                    kp1 = k+1;
                    km1 = k-1;
                    l = n-k;
                    ap::vmove(work.getvector(1, l), a.getcolumn(k, kp1, n));
                    symmetricmatrixvectormultiply(a, isupper, k+1, n, work, double(-1), work2);
                    ap::vmove(a.getcolumn(k, kp1, n), work2.getvector(1, l));
                    v = ap::vdotproduct(&work(1), &work2(1), ap::vlen(1,l));
                    a(k,k) = a(k,k)-v;
                }
                kstep = 1;
            }
            else
            {
                
                //
                // 2 x 2 diagonal block
                //
                // Invert the diagonal block.
                //
                t = fabs(a(k,k-1));
                ak = a(k-1,k-1)/t;
                akp1 = a(k,k)/t;
                akkp1 = a(k,k-1)/t;
                d = t*(ak*akp1-1);
                a(k-1,k-1) = akp1/d;
                a(k,k) = ak/d;
                a(k,k-1) = -akkp1/d;
                
                //
                // Compute columns K-1 and K of the inverse.
                //
                if( k<n )
                {
                    kp1 = k+1;
                    km1 = k-1;
                    l = n-k;
                    ap::vmove(work.getvector(1, l), a.getcolumn(k, kp1, n));
                    symmetricmatrixvectormultiply(a, isupper, k+1, n, work, double(-1), work2);
                    ap::vmove(a.getcolumn(k, kp1, n), work2.getvector(1, l));
                    v = ap::vdotproduct(&work(1), &work2(1), ap::vlen(1,l));
                    a(k,k) = a(k,k)-v;
                    v = ap::vdotproduct(a.getcolumn(k, kp1, n), a.getcolumn(km1, kp1, n));
                    a(k,k-1) = a(k,k-1)-v;
                    ap::vmove(work.getvector(1, l), a.getcolumn(km1, kp1, n));
                    symmetricmatrixvectormultiply(a, isupper, k+1, n, work, double(-1), work2);
                    ap::vmove(a.getcolumn(km1, kp1, n), work2.getvector(1, l));
                    v = ap::vdotproduct(&work(1), &work2(1), ap::vlen(1,l));
                    a(k-1,k-1) = a(k-1,k-1)-v;
                }
                kstep = 2;
            }
            kp = abs(pivots(k));
            if( kp!=k )
            {
                
                //
                // Interchange rows and columns K and KP in the trailing
                // submatrix A(k-1:n,k-1:n)
                //
                if( kp<n )
                {
                    l = n-kp;
                    kp1 = kp+1;
                    ap::vmove(work.getvector(1, l), a.getcolumn(k, kp1, n));
                    ap::vmove(a.getcolumn(k, kp1, n), a.getcolumn(kp, kp1, n));
                    ap::vmove(a.getcolumn(kp, kp1, n), work.getvector(1, l));
                }
                l = kp-k-1;
                i1 = k+1;
                i2 = kp-1;
                ap::vmove(work.getvector(1, l), a.getcolumn(k, i1, i2));
                ap::vmove(a.getcolumn(k, i1, i2), a.getrow(kp, i1, i2));
                ap::vmove(&a(kp, i1), &work(1), ap::vlen(i1,i2));
                temp = a(k,k);
                a(k,k) = a(kp,kp);
                a(kp,kp) = temp;
                if( kstep==2 )
                {
                    temp = a(k,k-1);
                    a(k,k-1) = a(kp,k-1);
                    a(kp,k-1) = temp;
                }
            }
            k = k-kstep;
        }
    }
    return result;
}


/*************************************************************************
Obsolete 1-based subroutine
*************************************************************************/
bool inversesymmetricindefinite(ap::real_2d_array& a, int n, bool isupper)
{
    bool result;
    ap::integer_1d_array pivots;

    ldltdecomposition(a, n, isupper, pivots);
    result = inverseldlt(a, pivots, n, isupper);
    return result;
}



